import time
import platform
import numpy as np
from . import gpio

class Servo():
    def __init__(self,config):
        super().__init__()
        self.data=[]
        self.deg=0
        self.config=config
        pass

    def initRapi(self,config):
        if platform.machine()=="aarch64":
            import RPi.GPIO as GPIO
            GPIO.setmode(GPIO.BCM)
            GPIO.setup(config["pin"], GPIO.OUT)
            pwm = GPIO.PWM(config["pin"],50)
            pwm.start(config["min"])
            return pwm

    def initOther(self,config):
        return gpio.GPIO()
     
    def change(self,deg):
        pwm = self.initRapi(self.config) if platform.machine()=="aarch64" else self.initOther(self.config)
        pwm.ChangeDutyCycle(deg)
        time.sleep(0.08)

    def append(self,duty):
        if (duty==65535):return
        self.data.append(duty)
        pass

    def compute(self):
        data=np.array(self.data)
        z_scores = np.abs((data - np.mean(data))/np.std(data))
        duty =  np.mean(data[z_scores < 20])
        duty = abs(self.config["deg"]-((duty)/(self.config["max"]-self.config["min"]))*180)
        deg = round(2.5+(duty/180)*10, 1)
        self.data.clear()
        return deg

    def start(self):
        deg=self.compute()
        if abs(deg-self.deg)>0.3:
            self.change(deg)
            self.deg=deg
